/*
 *
 * Copyright (C) 2014
 * Julio Jarquin, Gustavo Arechavaleta <garechav@cinvestav.edu.mx>
 * CINVESTAV - Saltillo Campus
 *
 * This file is part of HRLocomotion-1.0
 * HRLocomotion-1.0 free software; you can redistribute it and/or
 * modify it under the terms of the GNU Lesser General Public
 * License as published by the Free Software Foundation; either
 * version 2.1 of the License, or (at your option) any later version.
 *
 * HRLocomotion-1.0 is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * Lesser General Public License for more details.

 * You should have received a copy of the GNU Lesser General Public
 * License along with this library; if not, write to the Free Software
 * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
 * 02110-1301  USA
 */

#include "HRLocomotion/mpc_locomotion.h"
#include "qpOASES.hpp"
#include "qpOASES/SQProblem.hpp"
#include <iostream>


/*  Steps: */


/*! Initialize the constant members with constant or initial values. */ // DONE
/*! Initialize the matrices Pvs, Pvu, Pzs, Pzu. */   // DONE
/*! Initialize the matrices Uc and Ur. */ //DONE
/*! Initialize the matrices Q' and Q. */ //DONE
/*! Set the pk vector for the i period(0-7). */ //DONE


/*! Init restrictions. */
/*! Set restrictions. */
/*! Init QP. */  //Solve initial SQP
/*! getInitialPosture */
/*! LOOP: solveWalking */
/*!     getActualState*/
/*!     solveQP */
/*!         setVectorPk(stage);
/*!         setRestrictions();
/*!         quadprog.hotstart */
/*!        -----------------End of MPC */
/*!         getNextState */
/*!         footTrajectory */
/*!         setTasks() */
/*! END LOOP: solveWalking */

using std::cout;
using std::endl;



namespace hrLocomotion{

/*! Motion constructor */

    MPCLocomotion::MPCLocomotion(): g(9.80665), h(0.315), alfa(10e-6), beta(1), gamma(1),
        Td(0.1), m(2), Ss(8), xfc(0), yfc(0.09), xpref(0.01), ypref(0.15),
        actual(true), stage(0), walking(false), theta(0) {

        N = m*Ss;  //Horizon
        initMatricesP();
        initMatricesU();
        std::cout << "Setting Q" << std::endl;
        initMatricesQ();

        ones.setOnes(N);
        x_ac.setZero(3);
        y_ac.setZero(3);
        y_ac(0) = 0.09;
        pk.resize(2*(N+m));
        setVectorPk(0);
        initRestrictions();


        //Dynamic System Matrices
        A_din.resize(3,3);
        A_din.setIdentity();
        A_din(0,1) = A_din(1,2) = Td;
        A_din(0,2) = pow(Td,2)/2;
        B_din.resize(3);
        B_din(0) = pow(Td,3)/6;
        B_din(1) = pow(Td,2)/2;
        B_din(2) = Td;

        //Control
        uk.resize(2*(N+m));
        R.resize(2,2);   //I guess is for the rotation

        setRestrictions();
        //quadprog.reset(new qpOASES::SQProblem(10,10));
    }


bool MPCLocomotion::init(){
    nWSR = 1000;
    nV = 2*(N+m);
    nC = b_.rows();
    qpOASES::Options options;
    options.setToMPC();
    options.printLevel = qpOASES::PL_NONE;
    quadprog.reset(new qpOASES::SQProblem(nV,nC));
    quadprog->setOptions(options);
    quadprog->init(Q[0].data(), pk.data(),A_.data(),NULL,NULL,NULL,b_.data(),nWSR);
    if(quadprog->getPrimalSolution(uk.data()) == qpOASES::RET_QP_NOT_SOLVED){
        return FAILED_INIT;
    }
        return SUCCESS_INIT;
}

 //default templates not working here :(
VectorXr MPCLocomotion::getU(){
    return uk;
}

//Just for compatibility issues, to be redefined
//
void MPCLocomotion::setInitialPosture(real_t x, real_t y){
    x_ac(0) = x;
    y_ac(0) = y;

    x_next = x_ac;
    y_next = y_ac;
}

void MPCLocomotion::solveNext(){
    setVectorPk(stage);
    setRestrictions();
    nWSR = 1000;
    /// NOTE solve qp: CON res.                                                     ** el 0 es de 2.0
    quadprog->hotstart(Q[stage].data(), pk.data(),A_.data(),NULL,NULL,NULL,b_.data(),nWSR);/// CON RESTRICCIONES
    quadprog->getPrimalSolution(uk.data());
}














/*! Initialize the matrices Pvs, Pvu, Pzs, Pzu. */
void MPCLocomotion::initMatricesP()
{
    // setting Pvs
    Pvs.resize(N,3);
    for(int i=0; i!=N; i++){
        Pvs.row(i) << 0, 1, (i+1)*Td;
    }

    // setting Pvu
    Pvu.setZero(N,N);
    VectorXr tempPvu(N,1);
    for(int i=0; i!=N; i++){
        tempPvu(i) = (1+2*i)*pow(Td,2)/2;
    }
    for(int i=N, j=0; i>0; i--, j++){
        Pvu.col(j).tail(i) = tempPvu.head(i);
    }

    // setting Pzs
    Pzs.resize(N,3);
    for(int i=0; i!=N; i++){
        Pzs.row(i) << 1, (i+1)*Td, (pow((double)(i+1),2)*pow(Td,2)/2)-(h/g);
    }

    // setting Pzu
    Pzu.setZero(N,N);
    VectorXr tempPzu(N,1);
    for(int i=0; i!=N; i++){
        tempPzu(i) = ((1+3*i+3*pow((double)i,2))*pow(Td,3)/6)-(Td*h/g);
    }
    for(int i=N, j=0; i>0; i--, j++){
        Pzu.col(j).tail(i) = tempPzu.head(i);
    }

}

/*! Initialize the matrices Uc and Ur. */
void MPCLocomotion::initMatricesU()
{
    // setting Uc
    std::cout << "Setting UC" <<std::endl;
    VectorXr tempUc = VectorXr::Zero(N,1);
    for(int i=0; i!=Ss; i++){
        Uc.push_back(tempUc);
    }
    std::cout << "Setting UC 2" <<std::endl;
    for(int i=0, j=Ss-1; i<Ss-1; i++, j--){
        Uc[i].head(j).setConstant(1);
    }
    std::cout << "Setting UR" <<std::endl;

    // setting Ur
    MatrixXr tempUr = MatrixXr::Zero(N,m);
    for(int i=0; i!=Ss; i++){
        Ur.push_back(tempUr);
    }

    std::cout << "Setting UR 2" <<std::endl;
    for(int i=0; i!=Ss; i++){
        int col=0;
        int con=0;
        for(int j=Ss-1-i; j<N; j++){
            Ur[i](j,col) = 1;
            con++;
            if(con==8){
                col++;
                con = 0;
            }
        }
    }

}

/*! Initialize the matrices Q' and Q. */
void MPCLocomotion::initMatricesQ()
{
    MatrixXr Qtes(N+m,N+m), blockzero(N+m,N+m), eye(N,N), Qaux(2*(N+m),2*(N+m));
    blockzero.setZero();
    eye.setIdentity();
    cout << "Setting Qtes" << endl;
    for(int i=0; i!=Ss; i++){
        cout << "I: " << i + 1 << endl;
        Qtes << (alfa*eye + beta*(Pvu.transpose()*Pvu) + gamma*(Pzu.transpose()*Pzu)),
                (-gamma*(Pzu.transpose()*Ur[i])), (-gamma*(Ur[i].transpose()*Pzu)),
                (gamma*(Ur[i].transpose()*Ur[i]));
        Qaux << Qtes, blockzero, blockzero, Qtes;
        Q.push_back(Qaux);
    }
}

/*! Set the pk vector for the i period(0-7). */
void MPCLocomotion::setVectorPk(int i)
{
    //I dont like the xpref, ypref, xfc, yfc, x_ac, y_ac, ones thing
    pk << beta*Pvu.transpose()*(Pvs*x_ac-xpref*ones) + gamma*Pzu.transpose()*(Pzs*x_ac-Uc[i]*xfc),
            -gamma*Ur[i].transpose()*(Pzs*x_ac-Uc[i]*xfc),
            beta*Pvu.transpose()*(Pvs*y_ac-ypref*ones) + gamma*Pzu.transpose()*(Pzs*y_ac-Uc[i]*yfc),
            -gamma*Ur[i].transpose()*(Pzs*y_ac-Uc[i]*yfc);
}

/*! Init restrictions. */
void MPCLocomotion::initRestrictions()
{
    margin = sqrt((pow((0.038*.9), 2))/2);
    points.resize(2,4);
    minFootSep = 0.09;
    A_pl.resize(5,2);
    A_pr.resize(5,2);
    A_p.setZero(5,2*(N+m));
    b_p.resize(5);
    b_paux.resize(5);
    b_paux << -0.09, 5.2857, 8, 8, 5.2857;
    A_pl << 0,      1,
            100,     -14.2857,
            25,   -50,
            -25,    -50,
            -100,   -14.2857;
    A_pr << 0,      -1,
            100,     14.2857,
            25,   50,
            -25,    50,
            -100,   14.2857;


    b_zaux = b_zaux.setOnes(4*N)*margin;
    b_z.resize(4*N);
    D_z.resize(4*N,2*N);
    D_z << MatrixXr::Zero(N,N), MatrixXr::Identity(N,N)*(-1),
           MatrixXr::Identity(N,N), MatrixXr::Zero(N,N),
           MatrixXr::Zero(N,N), MatrixXr::Identity(N,N),
           MatrixXr::Identity(N,N)*(-1), MatrixXr::Zero(N,N);

    MatrixXr aux;
    aux.resize(2*N,2*(N+m));
    for(int i=0; i<Ss; i++){
        aux << Pzu, -Ur[i], MatrixXr::Zero(N,N+m), MatrixXr::Zero(N,N+m), Pzu, -Ur[i];
        A_z.push_back(D_z*aux);
    }

    /// NOTE generacion de restricciones: POSICION INCOMPLETA
    /*** Defining size for the constraints matrices ***/
    A_.resize(A_z[0].rows()+A_p.rows(),2*(N+m));
    b_.resize(b_z.rows()+b_p.rows());

//    A_.resize(A_p.rows(),2*(N+m));
//    b_.resize(b_p.rows());
//    A_.resize(A_z[0].rows(),2*(N+m));
//    b_.resize(b_z.rows());
//    A_.resize(4,2*(N+m));
//    b_.resize(4);
}

/*! Set restrictions. */
void MPCLocomotion::setRestrictions()
{
    //******** setting position constraint
    R << cos(theta),-sin(theta),sin(theta),cos(theta);
    if(actual){
        points << 0.11,0.11,-0.047,-0.047,-0.05,0.038,0.038,-0.05;
        rpoints = R*points;
        minStepY = minFootSep - (points.row(1).maxCoeff() - rpoints.row(1).maxCoeff());
        b_paux(0) = -minStepY;
        b_p = b_paux + A_pl.col(0)*xfc + A_pl.col(1)*yfc;
        A_p.col(N) = A_pl.col(0);
        A_p.col(N+m+N) = A_pl.col(1);
    }
    else{
        points << 0.11,0.11,-0.047,-0.047,-0.038,0.050,0.050,-0.038;
        rpoints = R*points;
        minStepY = minFootSep + (points.row(1).minCoeff() - rpoints.row(1).minCoeff());
        b_paux(0) = -minStepY;
        b_p = b_paux + A_pr.col(0)*xfc + A_pr.col(1)*yfc;
        A_p.col(N) = A_pr.col(0);
        A_p.col(N+m+N) = A_pr.col(1);
    }
//    cout<<"min: "<<minStepY<<endl<<endl;
//    cout<<"xfc: "<<xfc<<endl;
//    cout<<"yfc: "<<yfc<<endl;
//    cout<<"bpaux"<<b_paux<<endl<<endl;
//    cout<<"b_p"<<endl<<b_p<<endl<<endl;

    //******** setting ZMP constraint
    VectorXr xfyf;
    xfyf.resize(2*N);
    xfyf << (Uc[stage]*xfc - Pzs*x_ac), (Uc[stage]*yfc - Pzs*y_ac);
    b_z = b_zaux + D_z*xfyf;

    /// NOTE seleccion de restricciones: POSICION INCOMPLETA
    //******* setting the complete constraints
    A_ << A_p, A_z[stage];
    b_ << b_p, b_z;

//    A_ << A_p;
//    b_ << b_p;
//    A_ << A_z[1];
//    b_ << b_z;
//    A_ <<  A_p.block(1,0,4,2*(N+m));//Block of size (p,q), starting at (i,j) , matrix.block(i,j,p,q);
    //    b_ <<  b_p.segment(1,4);        //Block of n elements, starting at i , vector.segment(i,n);
}





 MPCLocomotion::~MPCLocomotion(){

 }


}



